#ifndef RVIZ_UTIL_PKG__RVIZ_MARKER_HPP_
#define RVIZ_UTIL_PKG__RVIZ_MARKER_HPP_

#include <memory>
#include <iostream>
#include <string>
#include <vector>
#include "rclcpp/rclcpp.hpp"
#include <rclcpp_lifecycle/lifecycle_node.hpp>
#include "visualization_msgs/msg/marker.hpp"
#include "geometry_msgs/msg/point.hpp"
#include "nav2_util/lifecycle_node.hpp"
// 自定义

namespace rviz_util_pkg
{

    class RvizMarker
    {

    public:
        RvizMarker(rclcpp::Node *node);
        RvizMarker(rclcpp_lifecycle::LifecycleNode::SharedPtr node);
        ~RvizMarker();
        //绘制点位
        void publish_marker_point(double x,double y, std::string name);
        //清除命名空间的所有点位
        void clear_ns_points(const std::string &ns_name);
        //绘制文本
        void publish_marker_txt(double x,double y, std::string name);
        //绘制线
        void publish_marker_line(double x1,double y1,double x2,double y2); 
    private:
        // Logger
        rclcpp::Logger logger_{rclcpp::get_logger("rviz_util_pkg")};

        rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr marker_point_pub_; // 点位
        rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr marker_txt_pub_;   // 文本
        rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr marker_line_pub_;  // 画线

        int marker_point_id_ = 0;
        int marker_txt_id_ = 0;
        int marker_line_id_ = 0;

        std::string global_frame_id_ = "map";
    };

}

#endif // RVIZ_UTIL_PKG__RVIZ_MARKER_HPP_